/*
 * Created on 23 janv. 2008
 *
 * TODO To change the template for this generated file go to
 * Window - Preferences - Java - Code Style - Code Templates
 */
package evolution;

import javax.vecmath.Vector3d;

import simbad.sim.Agent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.LampActuator;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;

/**
 * @author nancel
 *
 * TODO To change the template for this generated type comment go to
 * Window - Preferences - Java - Code Style - Code Templates
 */
public class Bat extends Agent {
	
	LampActuator lamp;
	
	EnvironmentDescription _environment;
	RangeSensorBelt sonars, bumpers;
	Perceptator perceptator;
	
	public static final int NB_ENTREES = 4;
	public static final int NB_COUCHES = 6;
	
	double translationVelocitySum = 0.0;
	double wallsDistanceSum = 0.0;
	
	int stepsNumber = 0;
	
	boolean active = true;
	
	public Bat(Vector3d position, String name) {
		super(position, name);
		
		// Add sensors
		
		sonars = RobotFactory.addSonarBeltSensor(this,NB_ENTREES);
		lamp = RobotFactory.addLamp(this);

		// bumpers = RobotFactory.addBumperBeltSensor(this,12);
		initBehavior();
		
	}
	
	public Bat(Vector3d position, String name, EnvironmentDescription env) {
		super(position, name);
		
		this._environment = env;
		
		// Add sensors
		
		sonars = RobotFactory.addSonarBeltSensor(this,NB_ENTREES);
		lamp = RobotFactory.addLamp(this);

		// bumpers = RobotFactory.addBumperBeltSensor(this,12);
		initBehavior();
	}
	
	/** Initialize Agent's Behavior */
	public void initBehavior() {
		active = true;
		stepsNumber=0;
		translationVelocitySum = 0.0;
		wallsDistanceSum = 0.0;
		perceptator = new Perceptator(NB_ENTREES, NB_COUCHES, 2);
		
	}
	
	/** Perform one step of Agent's Behavior */
	public void performBehavior() 
	{
		if( !active ) return;
//		if( collisionDetected() ){
//			active = false;
//			return;
//		}
		double[] entrees = new double[NB_ENTREES];
		double[] sorties = new double[2];
		
		double sum = 0;
		for (int i = 0; i < NB_ENTREES; i++){
			entrees[i] = sonars.getMeasurement(i);
			//System.out.println(sonars.getMeasurement(i));
			sum += sonars.getMeasurement(i);
		}
		wallsDistanceSum += sum/NB_ENTREES;
		
		sorties = perceptator.run(entrees, false);
		
		setTranslationalVelocity(sorties[0]);
		setRotationalVelocity(sorties[1]);
		
		translationVelocitySum += getTranslationalVelocity();
		
		stepsNumber++;
	}
	
	double absoluteWheelsVelocity(){
		return translationVelocitySum / stepsNumber;
	}
	
	double distance(){
		return getOdometer();
	}
	
	double wallsDistance(){
		return wallsDistanceSum / stepsNumber;
	}
	
	double fitness(){
		return absoluteWheelsVelocity() + distance() + wallsDistance();
	}
	
	
	public void setPerceptatorPoids(double[] poids){
		perceptator.setPoids(poids);
	}
	
}
